#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

/*
订阅方：订阅发布的相关坐标系转换关系， 传入一个坐标点，调用tf实现转换

流程：
1，包含头文件
2，编码，初始化节点， 创建节点句柄
3，创建订阅对象； -->订阅在坐标系相对关系
4，组织一个坐标点数据
5，转换算法，调用TF内置函数实现
6，最后输出；
*/
int main(int argc, char ** argv)
{

    //2，编码，初始化节点， 创建节点句柄
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle nh;
    //3，创建订阅对象； -->订阅在坐标系相对关系
    //3.1, 创建一个buffer
    tf2_ros::Buffer buffer;
    //3.2, 创建监听对象，（将所有的订阅信息存入buffer）
    tf2_ros::TransformListener listener(buffer);
    //4, 组织一个坐标点
    geometry_msgs::PointStamped ps;
    ps.header.frame_id="larse";
    ps.header.stamp=ros::Time::now();
    ps.point.x=2.0;
    ps.point.y=2.0;
    ps.point.z=2.0;
    //添加休眠
    ros::Duration(2).sleep();
    //转换算法， 调用tf内置实现
    ros::Rate rate(10);

    while(ros::ok())
    {
        //核心代码将 ps 转换为相对于 base_link 的座标点
        geometry_msgs::PointStamped ps_out;
        /*
            调用了buffer的转换函数， transform
            参数1：被转换的坐标点
            参数2：目标坐标系
            返回值：输出的坐标点
            
            ps1: 调用时必须包含头文件： tf2_geometry_msgs/tf2_geometry_msgs.h
            ps2：运行时抛出异常 base_link 不存在
            原因：订阅数据是一个耗时操作， 可能调用transform时坐标系的相对关系还未订阅到
            解决：方案一：调用函数前，执行休眠
                 方案二：进行异常处理
        */

       try{
            ps_out=buffer.transform(ps,"base_link");
            //结果输出
            ROS_INFO("转换后的座标点 %2f, %2f, %2f", ps_out.point.x,
            ps_out.point.y, ps_out.point.z);

       }
       catch(const std::exception& e)
       {
           //std::cerr<<e.what()<<'/n';
           ROS_INFO("异常消息： %s", e.what());
       }
  
        rate.sleep();
        ros::spinOnce();
    }

    return 0;
}
